#include "remote_task.h"
RC_Ctrl_t RC_CtrlData;
InputMode_e InputMode;
Gimbal_Ref_t GimbalRef;
ChassisSpeed_Ref_t ChassisSpeedRef;
void SetInputMode(Remote *rc)
{
	switch(rc->s2)
	{
		case 1: InputMode = REMOTE_INPUT; break;
		case 3: InputMode = STOP; break;
		case 2: InputMode = KEY_MOUSE_INPUT; break;
	}
}
void RemoteDataPrcess(uint8_t *pData)
{
	RC_CtrlData.rc.ch0 = ((int16_t)(pData[0] | pData[1]<<8)) & 0x7FF;
	RC_CtrlData.rc.ch1 = (int16_t)(pData[1]>>3 | pData[2]<<5) & 0x7FF;
	RC_CtrlData.rc.ch2 = (int16_t)(pData[2]>>6 | pData[3]<<2|pData[4]<<10) & 0x7FF;
	RC_CtrlData.rc.ch3 = (int16_t)(pData[4]>>1 | pData[5]<<7) & 0x7FF;	
	RC_CtrlData.rc.ch4 = (pData[16] | (pData[17] << 8)) & 0x07FF; 
	RC_CtrlData.rc.s1 = (pData[5]>>4 & 0x000C) >> 2;
	RC_CtrlData.rc.s2 = pData[5]>>4 & 0x0003;
	
  RC_CtrlData.mouse.x = ((int16_t)pData[6]) | ((int16_t)pData[7] << 8);
  RC_CtrlData.mouse.y = ((int16_t)pData[8]) | ((int16_t)pData[9] << 8);
  RC_CtrlData.mouse.z = ((int16_t)pData[10]) | ((int16_t)pData[11] << 8);    

  RC_CtrlData.mouse.press_l = pData[12];
  RC_CtrlData.mouse.press_r = pData[13];
  RC_CtrlData.key.v = ((int16_t)pData[14]) | ((int16_t)pData[15] << 8);	
	SetInputMode(&RC_CtrlData.rc);
	switch(GetInputMode())
	{
		case REMOTE_INPUT: RemoteControlProcess(&RC_CtrlData.rc); break;
		case STOP: break;
		case KEY_MOUSE_INPUT: MouseKeyControlProcess(&RC_CtrlData.mouse,&RC_CtrlData.key); break;
	}
}
InputMode_e GetInputMode()
{
	return InputMode;
}

void RemoteControlProcess(Remote *rc)
{
	ChassisSpeedRef.forward_back_ref = rc->ch1 - (int16_t)REMOTE_CONTROLLER_STICK_OFFSET;
	ChassisSpeedRef.left_right_ref = rc->ch0 - (int16_t)REMOTE_CONTROLLER_STICK_OFFSET;
	GimbalRef.pitch_angle_dynamic_ref +=(rc->ch3 - (int16_t)REMOTE_CONTROLLER_STICK_OFFSET) * RMOTE_VALUE_UNITIZATION * PITCH_MAX_SPEED;
	GimbalRef.yaw_angle_dynamic_ref += (rc->ch2 - (int16_t)REMOTE_CONTROLLER_STICK_OFFSET) * RMOTE_VALUE_UNITIZATION * YAW_MAX_SPEED;
	if(rc->s1 == 1)
	{
		
	}
	if(rc->s1 == 3)
	{
		SetSportState(SPORT_STATE);
	}
	if(rc->s1 == 2)
	{
		SetSportState(ROTATION_STATE);
	}
}

void MouseKeyControlProcess(Mouse *mouse,Key *key)
{
	static uint16_t forward_back_speed = 0;
	static uint16_t left_right_speed = 0;	
	if(GetWorkState() != PREPARE_STATE)
	{
		if(key->v & KEY_SHIFT)
		{
			forward_back_speed = HIGH_FORWARD_BACK_SPEED;
			left_right_speed = HIGH_LEFT_RIGHT_SPEED;
		}
		else 
		{
			forward_back_speed = NORMAL_FORWARD_BACK_SPEED;
			left_right_speed = NORMAL_LEFT_RIGHT_SPEED;
		}
		if(key->v & KEY_W)
		{
			ChassisSpeedRef.forward_back_ref = -forward_back_speed;
		}
		else if(key->v & KEY_S)
		{
			ChassisSpeedRef.forward_back_ref = forward_back_speed;
		}
		if(key->v & KEY_D)
		{
			ChassisSpeedRef.left_right_ref	= left_right_speed;
		}
		else if(key->v & KEY_A)
		{
			ChassisSpeedRef.left_right_ref	= -left_right_speed;
		}
	}
}

